{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<style>/* div {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "} */\n",
       "\n",
       "\n",
       "/* .inner_cell {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "} */\n",
       "\n",
       ".cell {\n",
       "    /* max-width: 900px !important; */\n",
       "    /* line-height: 1.6em; */\n",
       "    padding-bottom: 15px;\n",
       "    text-align: justify;\n",
       "}\n",
       "\n",
       ".markup {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "}\n",
       "\n",
       "img {\n",
       "    display: block;\n",
       "    margin-left: auto;\n",
       "    margin-right: auto;\n",
       "}\n",
       "\n",
       "/* αt */\n",
       "/* βt */\n",
       "/* γt */</style>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Apply custon style to notebook\n",
    "from IPython.core.display import HTML\n",
    "import pathlib\n",
    "styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "styles = open(styles_path, \"r\").read()\n",
    "HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 7.0 Quadratic Rate Motion Control\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[7.1 Swift and Robot Setup](#1)\n",
    "\n",
    "[7.2 Quadratic Rate Motion Control](#2)\n",
    "* Singularity Escapability\n",
    "  * Quadratic-Rate Motion Control\n",
    "\n",
    "[7.3 Position based Servoing with QRMC](#3)\n",
    "\n",
    "[7.4 QRMC Finishing in a Singularity](#4)\n",
    "\n",
    "[7.5 QRMC Starting and Finishing in a Singularity](#5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes eith the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='1'></a>\n",
    "\n",
    "### 7.1 Swift and Robot Setup\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this notebook we will be using the Robotics Toolbox for Python and Swift to simulate our motion controllers. Check out Part 1 Notebook 3 for an introduction to these packages."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52010/?53010\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28b787190>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Make the environment\n",
    "env = Swift()\n",
    "\n",
    "# Launch the simulator, will open a browser tab in your default\n",
    "# browser (chrome is recommended)\n",
    "# The realtime flag will ask the simulator to simulate as close as\n",
    "# possible to realtime as apposed to as fast as possible\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "\n",
    "# Make a ur5 robot\n",
    "robot = rtb.models.UR5()\n",
    "\n",
    "# Set the joint coordinates to qr\n",
    "robot.q = robot.qr\n",
    "\n",
    "# We can then add our robot to the simulator envionment\n",
    "env.add(robot)\n",
    "\n",
    "# end-effector axes\n",
    "ee_axes = sg.Axes(0.1)\n",
    "\n",
    "# goal axes\n",
    "goal_axes = sg.Axes(0.1)\n",
    "\n",
    "# Add the axes to the environment\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='2'></a>\n",
    "\n",
    "### 7.2 Quadratic Rate Motion Control\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Quadratic-rate motion control is a method of controlling a manipulator through or near a singularity. By using the second-order differential kinematics, the controller does not break down at or near a singularity, like the resolved-rate motion controller would. We start by considering the end-effector pose $\\bf{x} = f(\\bf{q}) \\in \\mathbb{R}^6$ in terms of the forward kinematics.\n",
    "Introducing a small change to the joint coordinates $\\Delta{\\bf{q}}$, we can write\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{x} + \\Delta{\\bf{x}} &= f(\\bf{q} + \\Delta{\\bf{q}}).\n",
    "\\end{align*}\n",
    "\n",
    "and the Taylor series expansion is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{x} + \\Delta{\\bf{x}}\n",
    "    &= f(\\bf{q}) +\n",
    "    \\dfrac{\\partial f}{\\partial \\bf{q}} \\Delta{\\bf{q}} +\n",
    "    \\dfrac{1}{2}\n",
    "    \\left(\n",
    "        \\dfrac{\\partial^2 f}{\\partial \\bf{q}^2} \\Delta{\\bf{q}}\n",
    "    \\right) \\Delta{\\bf{q}} +\n",
    "    \\cdots \\\\\n",
    "    \\Delta{\\bf{x}}\n",
    "    &=\n",
    "    \\bf{J}(\\bf{q}) \\Delta{\\bf{q}} +\n",
    "    \\frac{1}{2}\n",
    "    \\Big(\n",
    "        \\bf{H}(\\bf{q}) \\Delta{\\bf{q}}\n",
    "    \\Big) \\Delta{\\bf{q}} +\n",
    "    \\cdots\n",
    "\\end{align*}\n",
    "\n",
    "where for quadratic-rate control we wish to retain both the linear and quadratic terms (the first two terms) of the expansion.\n",
    "\n",
    "To form our controller, we use an iteration-based Newton-Raphson approach (as we did with resolved-rate motion control), where $k$ indicates the current step. Firstly, we rearrange the above to\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{g}(\\Delta{\\bf{q}}_k)\n",
    "    &=\n",
    "    \\bf{J}(\\bf{q}_k) \\Delta{\\bf{q}}_k +\n",
    "    \\frac{1}{2}\n",
    "    \\Big(\n",
    "        \\bf{H}(\\bf{q}_k) \\Delta{\\bf{q}}_k\n",
    "    \\Big) \\Delta{\\bf{q}}_k -\n",
    "    \\Delta{\\bf{x}}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{q}_k$ and $\\Delta{\\bf{q}}_k$ are the manipulator's current joint coordinates and change in joint coordinates respectively, and $\\Delta{\\bf{x}}$ is the desired change in end-effector position. Taking the derivative of $\\bf{g}$ with respect to $\\Delta{\\bf{q}}$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\partial \\bf{g}(\\Delta{\\bf{q}}_k)}\n",
    "          {\\partial \\Delta{\\bf{q}}}\n",
    "    &=\n",
    "    \\bf{J}(\\bf{q}_k) + \\bf{H}(\\bf{q}_k)\\Delta{\\bf{q}}_k \\\\\n",
    "    &= \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)\n",
    "\\end{align*}\n",
    "\n",
    "we get a new Jacobian $\\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)$. Using this Jacobian we can create a new linear system\n",
    "\n",
    "\\begin{align*}\n",
    "    \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k) \\bf{\\delta}_{\\Delta{\\bf{q}}} &= \\bf{g}(\\Delta{\\bf{q}}_k) \\\\\n",
    "    \\bf{\\delta}_{\\Delta{\\bf{q}}} &= \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)^{-1} \\bf{g}(\\Delta{\\bf{q}}_k)\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{\\delta}_{\\Delta{\\bf{q}}}$ is the update to $\\Delta{\\bf{q}}_k$. The change in joint coordinates at the next step are\n",
    "\n",
    "\\begin{align*}\n",
    "    \\Delta{\\bf{q}}_{k+1} = \\Delta{\\bf{q}}_{k} - \\bf{\\delta}_{\\Delta{\\bf{q}}.}\n",
    "\\end{align*}\n",
    "\n",
    "In the case that $\\Delta{\\bf{q}} = \\bf{0}$, quadratic-rate control reduces to resolved-rate control. This is not suitable if the robot is in or near a singularity. Therefore, a value near $0$ may be used to seed the initial value with $\\Delta{\\bf{q}} = \\bf{0.1}$, or the pseudo-inverse approach could be used\n",
    "\n",
    "\\begin{align*}\n",
    "    \\Delta{\\bf{q}} = \\bf{J}(\\bf{q})^{+} \\Delta{\\bf{x}}.\n",
    "\\end{align*}\n",
    "\n",
    "This controller can be used in the same manner as resolved-rate motion control described in Part 1. The manipulator Jacobian and Hessian are calculated in the robot's base frame, and the end-effector velocity $\\Delta{\\bf{x}}$ can be calculated using the angle-axis method described in Part 1.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets make it happen in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52011/?53011\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x10748ff10>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the environment, launch and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(robot)\n",
    "env.add(ee_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "robot.q = np.array([0, -np.pi / 2.2, np.pi / 2, 0.0, -0.0, 0])\n",
    "robot.qd = np.zeros(robot.n)\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# Specify our desired end-effector velocity\n",
    "ev = [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.005\n",
    "\n",
    "# Run the simulation for 4 seconds\n",
    "for _ in range(400):\n",
    "\n",
    "    # Work out the manipulator Jacobian, Hessian using the\n",
    "    # current robot configuration\n",
    "    J = robot.jacob0(robot.q)\n",
    "    H = robot.hessian0(robot.q)\n",
    "    \n",
    "    # Check if the joint velocity vector norm is close to zero\n",
    "    # If it is close to 0, use the pseudoinverse technique\n",
    "    # outlined above\n",
    "    if np.allclose(np.zeros(robot.n), robot.qd):\n",
    "        qd = np.linalg.pinv(J) @ ev\n",
    "    else:\n",
    "        qd = robot.qd\n",
    "\n",
    "    # Calculate H * qd\n",
    "    Hq = np.tensordot(H, qd, axes=(0, 0))\n",
    "\n",
    "    # Calculate the new Jacobian Jhat\n",
    "    Jhat = J + Hq\n",
    "\n",
    "    # Calculate the g vector\n",
    "    g = J @ qd + 0.5 * Hq @ qd - ev\n",
    "\n",
    "    # Calculate δq, the change to qd\n",
    "    δq = np.linalg.inv(Jhat) @ g\n",
    "\n",
    "    robot.qd = qd - δq\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = robot.fkine(robot.q)\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next example we use the end-effector frames of the Jacobian and Hessian and rotate around the z-axis"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52012/?53012\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28ec65910>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the environment, launch and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(robot)\n",
    "env.add(ee_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n",
    "robot.qd = np.zeros(robot.n)\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# Specify our desired end-effector velocity\n",
    "ev = [0.0, 0.0, 0.0, 0.0, 0.0, 0.5]\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.005\n",
    "\n",
    "# Run the simulation for 4 seconds\n",
    "for _ in range(400):\n",
    "\n",
    "    # Work out the manipulator Jacobian, Hessian using the\n",
    "    # current robot configuration\n",
    "    J = robot.jacobe(robot.q)\n",
    "    H = robot.hessiane(robot.q)\n",
    "    \n",
    "    # Check if the joint velocity vector norm is close to zero\n",
    "    # If it is close to 0, use the pseudoinverse technique\n",
    "    # outlined above\n",
    "    if np.allclose(np.zeros(robot.n), robot.qd):\n",
    "        qd = np.linalg.pinv(J) @ ev\n",
    "    else:\n",
    "        qd = robot.qd\n",
    "\n",
    "    # Calculate H * qd\n",
    "    Hq = np.tensordot(H, qd, axes=(0, 0))\n",
    "\n",
    "    # Calculate the new Jacobian Jhat\n",
    "    Jhat = J + Hq\n",
    "\n",
    "    # Calculate the g vector\n",
    "    g = J @ qd + 0.5 * Hq @ qd - ev\n",
    "\n",
    "    # Calculate δq, the change to qd\n",
    "    δq = np.linalg.inv(Jhat) @ g\n",
    "\n",
    "    robot.qd = qd - δq\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = robot.fkine(robot.q)\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='3'></a>\n",
    "\n",
    "### 7.3 Position based Servoing with QRMC\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As with previous motion controllers we have developed (Part 1 Notebook 3, Part 2 Notebooks 4, 5) this motion controller is best used with position-based servoing."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52013/?53013\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28b8592d0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the environment, launch and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(robot)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) \n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n",
    "robot.qd = np.zeros(robot.n)\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "Tep = (\n",
    "    robot.fkine(robot.q)\n",
    "    * sm.SE3.Tx(0.2)\n",
    "    * sm.SE3.Ty(0.2)\n",
    "    * sm.SE3.Tz(0.2)\n",
    "    * sm.SE3.Rx(-np.pi / 2)\n",
    "    * sm.SE3.Rz(-np.pi / 2)\n",
    ")\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.005\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "k = np.ones(6)\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = robot.fkine(robot.q).A\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    # Work out the manipulator Jacobian, Hessian using the\n",
    "    # current robot configuration\n",
    "    J = robot.jacob0(robot.q)\n",
    "    H = robot.hessian0(robot.q)\n",
    "    \n",
    "    # Check if the joint velocity vector norm is close to zero\n",
    "    # If it is close to 0, use the pseudoinverse technique\n",
    "    # outlined in Section 7.1\n",
    "    if np.allclose(np.zeros(robot.n), robot.qd):\n",
    "        qd = np.linalg.pinv(J) @ ev\n",
    "    else:\n",
    "        qd = robot.qd\n",
    "\n",
    "    # Calculate H * qd\n",
    "    Hq = np.tensordot(H, qd, axes=(0, 0))\n",
    "\n",
    "    # Calculate the new Jacobian Jhat\n",
    "    Jhat = J + Hq\n",
    "\n",
    "    # Calculate the g vector\n",
    "    g = J @ qd + 0.5 * Hq @ qd - ev\n",
    "\n",
    "    # Calculate δq, the change to qd\n",
    "    δq = np.linalg.inv(Jhat) @ g\n",
    "\n",
    "    robot.qd = qd - δq\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='4'></a>\n",
    "\n",
    "### 7.4 QRMC Finishing in a Singularity\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this example the robot must finish its motion at a singularity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52014/?53014\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28ecd04d0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the environment, launch and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(robot)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) \n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n",
    "robot.qd = np.zeros(robot.n)\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "Tep = robot.fkine(np.array([np.pi, 0.0, 0.0, 0.0, np.pi/2, 0.0]))\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.005\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "k = np.ones(6)\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = robot.fkine(robot.q).A\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    # Work out the manipulator Jacobian, Hessian using the\n",
    "    # current robot configuration\n",
    "    J = robot.jacob0(robot.q)\n",
    "    H = robot.hessian0(robot.q)\n",
    "    \n",
    "    # Check if the joint velocity vector norm is close to zero\n",
    "    # If it is close to 0, use the pseudoinverse technique\n",
    "    # outlined in Section 7.1\n",
    "    if np.allclose(np.zeros(robot.n), robot.qd):\n",
    "        # qd = np.linalg.pinv(J) @ ev\n",
    "        qd = 0.1 * np.ones(robot.n)\n",
    "    else:\n",
    "        qd = robot.qd\n",
    "\n",
    "    # Calculate H * qd\n",
    "    Hq = np.tensordot(H, qd, axes=(0, 0))\n",
    "\n",
    "    # Calculate the new Jacobian Jhat\n",
    "    Jhat = J + Hq\n",
    "\n",
    "    # Calculate the g vector\n",
    "    g = J @ qd + 0.5 * Hq @ qd - ev\n",
    "\n",
    "    # Calculate δq, the change to qd\n",
    "    δq = np.linalg.inv(Jhat) @ g\n",
    "\n",
    "    robot.qd = qd - δq\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='5'></a>\n",
    "\n",
    "### 7.5 QRMC Starting and Finishing in a Singularity\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this example the robot must start and finish its motion at a singularity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52015/?53015\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28ff6d410>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the environment, launch and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(robot)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration\n",
    "robot.q = np.zeros(robot.n)\n",
    "robot.qd = np.zeros(robot.n)\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "Tep = robot.fkine([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.005\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "k = np.ones(6)\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = robot.fkine(robot.q).A\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    # Work out the manipulator Jacobian, Hessian using the\n",
    "    # current robot configuration\n",
    "    J = robot.jacob0(robot.q)\n",
    "    H = robot.hessian0(robot.q)\n",
    "    \n",
    "    # Check if the joint velocity vector norm is close to zero\n",
    "    # If it is close to 0, use the pseudoinverse technique\n",
    "    # outlined in Section 7.1\n",
    "    if np.allclose(np.zeros(robot.n), robot.qd):\n",
    "        # qd = np.linalg.pinv(J) @ ev\n",
    "        qd = 0.1 * np.ones(robot.n)\n",
    "    else:\n",
    "        qd = robot.qd\n",
    "\n",
    "    # Calculate H * qd\n",
    "    Hq = np.tensordot(H, qd, axes=(0, 0))\n",
    "\n",
    "    # Calculate the new Jacobian Jhat\n",
    "    Jhat = J + Hq\n",
    "\n",
    "    # Calculate the g vector\n",
    "    g = J @ qd + 0.5 * Hq @ qd - ev\n",
    "\n",
    "    # Calculate δq, the change to qd\n",
    "    δq = np.linalg.inv(Jhat) @ g\n",
    "\n",
    "    robot.qd = qd - δq\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.9.5 ('rtb')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
